#include <math.h>

#define max(a, b) (a > b ? a : b)
#define min(a, b) (a < b ? a : b)
#define sgn(a) (((a)<0)? -1 : 1)

struct Point{
    double x,y;
};


struct MassPoint{
    double px,py;
    double ox,oy;
    double ax,ay;
    double vx,vy;
    int isFrozen;
    double mass;
};

struct Edge{
    struct MassPoint *v1;
    struct MassPoint *v2;
    double originalLength;
    int isSelected;
    struct Body *parent;
};



#define MAX_BODY_EDGES 128
#define MAX_BODY_VERTS 64
struct Body{
    int vertCount;
    int edgeCount;
    struct Edge *edges[MAX_BODY_EDGES];
    struct MassPoint *verts[MAX_BODY_VERTS];
    int hasCollided;
    int isFrozen;
    struct Point center;
    double MinX, MinY, MaxX, MaxY; //Min/max coordinates of the bounding box
};
typedef struct MassPoint mp;



#define MAX_POINTS 512
#define MAX_EDGES 1024
#define MAX_BODIES 64

static struct MassPoint pts[MAX_POINTS];
static int pointCount = 0;

static struct Edge edges[MAX_EDGES];
static int edgeCount = 0;

static struct Body bodies[MAX_BODIES];
static int bodyCount = 0;

static struct Edge *dragEdges[MAX_BODY_EDGES];

// added for collision normal info
static double minDistance = 0;
static double collisionDepth = 0;
static double collisionNormalX = 0;
static double collisionNormalY = 0;
static struct Edge *collisionEdge = 0x0;
static mp *collisionVert = 0x0;
static struct Body *collisionEdgeParent = 0x0;

// Gui state
static double linestarted=0;
static double ox=0,oy=0;
static int grabi = -1;
static mp *grabPtr = 0x0;
enum enumSelectionType{
    none, copy, makebody, deleteSelection
};

// shoot mouse
static double mousDirX, mousDirY;
static double oldmousx,oldmousy;

// Dt
static double dt = 0.01;
static double oldtim;

// Cam
static double zoomg=0.0;
static double transx=0;
static double transy=0;

#define drawcone drawCone
#define moveto moveTo
#define lineto lineTo

#define cls() glClear(GL_COLOR_BUFFER_BIT)
#define setcol glColor3ub


void inputMakePoint();
void inputDragPoint();

struct MassPoint *makPoint(double x1,double y1);
mp *findClosest(double x,double y,double *closestD);

void makeBox(double x,double y,double w,double h);
void makeCenteredBox(double x,double y,double s);
void makeWheel(double x, double y, double r);
struct Edge *makEdge(double x1,double y1,double x2,double y2);
struct Edge *makEdge2(struct MassPoint *p1, struct MassPoint *p2);

void UpdateVerlet(double dt);
void constrainPoints();
void IterateCollisions();
void calcBodyCenter();
void updateEdges();

void checkCollision();
double intervalDistance(double minA,double maxA, double minB,double maxB);
void projectToAxis(struct Body *b,double axisx,double axisy,double *mini,double *maxi);
int detectCollision(struct Body *body1, struct Body *body2);
void handleCollide(struct Body *a, struct Body *b);
int BodiesOverlap( struct Body* B1, struct Body* B2 ); //Used for optimization to test if the bounding boxes of two bodies overlap

int RectCollide(double testx,double testy,double rx,double ry,double w,double h);
void findEdgesInRect(int markAsSelected);
void makeCopy(double isCopy);
void makePhysBodyFromSelected();

void drawBodies();

static double selstarted = 0;

double distance2D(double a, double b){
        return sqrt(a*a+b*b);
}

/* Make a pointmass - ctor */
struct MassPoint *makPoint(double x1,double y1)
{
   if( pointCount >= MAX_POINTS ) return 0x0;
   pts[pointCount].px = x1; pts[pointCount].py = y1;
   pts[pointCount].ox = x1; pts[pointCount].oy = y1; // make velocity zero by setting now and old same
   pts[pointCount].isFrozen = 0;
   pts[pointCount].mass = 1.0;
   pointCount++;
   return &pts[pointCount-1];
}

/* Edge -ctor from two masspoints*/
struct Edge *makEdge2(struct MassPoint *p1, struct MassPoint *p2)
{
   if( p1==0x0 || p2==0x0 ) return 0x0;
   if( edgeCount >= MAX_EDGES ) return 0x0;
   struct Edge *e = &edges[edgeCount];
   e->v1 = p1; e->v2 = p2;
   e->originalLength = distance2D( (p1->px - p2->px), (p1->py - p2->py) ) ;
   e->parent = 0x0;
   edgeCount++;
   return e;
}

/* Edge -ctor from two masspoints*/
struct Edge *makBodyEdge(struct MassPoint *p1, struct MassPoint *p2)
{
    if( p1==0x0 || p2==0x0 ) return 0x0;
    if( edgeCount >= MAX_EDGES ) return 0x0;
    struct Edge *e = &edges[edgeCount];
    e->v1 = p1; e->v2 = p2;
    e->originalLength = distance2D( (p1->px - p2->px), (p1->py - p2->py) ) ;
    e->isSelected = makebody;
    e->parent = 0x0;
    edgeCount++;
    return e;
}

/* Edge -ctor from to points, merges with existing points */
struct Edge *makEdge(double x1,double y1,double x2,double y2)
{
   // if another point is close enough, merge with it.
   double closestD = 1e32; double snapDist = 10*10;
   mp *startPoint = findClosest(x1,y1, &closestD); // merge with start?
   if(startPoint && closestD < snapDist )
   {
      mp *endPoint = findClosest(x2,y2, &closestD); // merge with end? end also allready exists?
      if(endPoint && closestD < snapDist )
      {
         return makEdge2( startPoint, endPoint );
      }
      else // nop. make new point at end.
      {
         mp *newEnd = makPoint(x2,y2);
         return makEdge2( startPoint, newEnd );
      }
   }
   // did not merge with any, mak 2 new pts.
   return makEdge2( makPoint(x1,y1), makPoint(x2,y2) );
}

/* Find the closest masspoint to the point x,y. Return its indice*/
mp *findClosest(double x,double y,double *closestD)
{
   mp *closestPt = 0x0;
   *closestD = 1e32;
   for(int i=0; i<pointCount; i++)
   {
      double dSq = (pts[i].px-x)*(pts[i].px-x) + (pts[i].py-y)*(pts[i].py-y);

      if( dSq < *closestD ){
         *closestD = dSq;
         closestPt = &pts[i];
      }
   }

   return closestPt;
}

void makeCopy(double isCopy)
{
   ox=0;
   oy=0;
   double foundPt = 0;
   int oldEdgeCount = edgeCount;
   for(int i=0; i<oldEdgeCount; i++){
      if( edges[i].isSelected == copy ){
          struct Edge *blueprintEdge = &edges[i];
         //edges[i].isSelected = 0;
         double x1 = blueprintEdge->v1->px;
         double y1 = blueprintEdge->v1->py;
         double x2 = blueprintEdge->v2->px;
         double y2 = blueprintEdge->v2->py;

         if(foundPt==0)
         {
            foundPt = 1;
            ox = x1;
            oy = y1;
         }
         double tx = mousx-ox; double ty=mousy-oy; // translate to ox,oy
         struct Edge *newEd = makEdge(x1+tx,y1+ty,x2+tx,y2+ty);
         newEd->isSelected = makebody; // or not

         if( blueprintEdge->v1->isFrozen ){
             newEd->v1->isFrozen = 1;
         }
         if( blueprintEdge->v2->isFrozen){
             newEd->v2->isFrozen = 1;
         }
      }
   }
   makePhysBodyFromSelected();
}

/* take edges that have been marked as selected by users selection rectangle and make a phys body */
void makePhysBodyFromSelected()
{
    if(bodyCount >= MAX_BODIES) return;

      int vertsAdded = 0;
      int edgesAdded = 0;
      /* TODO optimise by not iterating thru all. put ones marked as makebody in a list */
      for(int i=0; i<edgeCount; i++)
      {
          struct Edge *e = &edges[i];
          if( e->isSelected == makebody )
          {
            e->parent = &bodies[bodyCount];
            bodies[bodyCount].isFrozen = 0;
            if( e->v1->isFrozen || e->v2->isFrozen ) bodies[bodyCount].isFrozen = 1;
            e->isSelected = 0;
            if( edgesAdded < (MAX_BODY_EDGES-1) && vertsAdded < (MAX_BODY_VERTS-1) )
            {
                bodies[bodyCount].edges[edgesAdded] = e; // copy ptr
                edgesAdded++;
                bodies[bodyCount].verts[vertsAdded+0] = e->v1;
                bodies[bodyCount].verts[vertsAdded+1] = e->v2;
                vertsAdded+=2;
           }
         }
      }
      bodies[bodyCount].edgeCount = edgesAdded;
      bodies[bodyCount].vertCount = vertsAdded;
      bodyCount++;
      calcBodyCenter();
}



void rotate2d(double *x, double *y, double angle)
{
   double x1 = *x;
   double y1 = *y;
   double sa = sin(angle);
   double ca = cos(angle);

   *x = x1 * ca - y1 * sa;
   *y = y1 * ca + x1 * sa;
}


void UpdateVerlet(double h)
{
   for(int i=0; i<pointCount; i++)
   {
      mp *p = &pts[i];
      if(p->isFrozen ) continue; // skip if frozen
      /*
      double tmpx = pts[i].px;
      double tmpy = pts[i].py;
      pts[i].px = 2*pts[i].px-pts[i].ox-h*h*pts[i].ax;
      pts[i].py = 2*pts[i].py-pts[i].oy-h*h*pts[i].ay;
      pts[i].vx = (pts[i].px - tmpx)/h;
      pts[i].vy = (pts[i].py - tmpy)/h;

      // Doesnt work this way.
      //vx[i] *= (exp(-fabs(vx[i])*0.01));
      //vy[i] *= (exp(-fabs(vy[i])*0.01));

      pts[i].vx *= (0.999);
      pts[i].vy *= (0.999);

      pts[i].ox = pts[i].px - h * pts[i].vx;
      pts[i].oy = pts[i].py - h * pts[i].vy;
      */

      p->vx = p->px - p->ox;
      p->vy = p->py - p->oy;
      p->ox = p->px;
      p->oy = p->py;
      p->px += p->vx - p->ax*h*h; // F=ma, a = f/M
      p->py += p->vy - p->ay*h*h;

      // resistance
      p->ox += p->vx * (1-0.9998);
      p->oy += p->vy * (1-0.9998);

      p->ax = 0;
      p->ay = -0.01;

   }
}

void drawBodies()
{
   setFont(4,6);
   for(int i=0; i<bodyCount; i++)
   {
       struct Body *b = &bodies[i];
      setcol(255,255,255);
      //glsettex( physBodyHandle[i] );
      glBegin(GL_LINE_LOOP); //glBegin(GL_POLYGON);
      // sumvxy find avg velocity of pointmasses
      double sumvx = 0; double sumvy = 0;
      for(int j=0; j<b->vertCount; j++)
      {
         //glTexCoord( physBodyTexcoords[i][j][0], physBodyTexcoords[i][j][1] );
         struct MassPoint *p = b->verts[j];
         glVertex2f( p->px, p->py );
         sumvx += p->vx;
         sumvy += p->vy;
      }
      glEnd();

      sumvx /= b->vertCount / 50.0;
      sumvy /= b->vertCount / 50.0;

      double len = distance2D(sumvx,sumvy);
      if(len > 25)
      {
         sumvx = 25*sumvx / len;
         sumvy = 25*sumvy / len;
      }


      // could sum up and find middle point
      setcol(255,255,0);
      drawCone(b->center.x,b->center.y,3.0,b->center.x+sumvx,b->center.y+sumvy,0.1);
      // Show boundingbox
      //setcol(255,0,255); DrawRect(b->MinX, b->MinY, b->MaxX - b->MinX, b->MaxY - b->MinY );
   }
}

void inputMakePoint(){
	// If this is the first point, then just remember it in (ox,oy) and say we've started drawing a line
   if( bstatus==1 && linestarted==0 )
   {
      bstatus = 0;
      linestarted = 1;
      ox = mousx;
      oy = mousy;

   }
   else if( bstatus==1 && linestarted==1 )
   {
      bstatus = 0;
      linestarted = 0;
      makEdge(ox,oy,mousx,mousy);
   }

/* hax frozen pt*/
   if( bstatus==4 && linestarted==0 )
   {
      bstatus = 0;
      linestarted = 1;
      ox = mousx;
      oy = mousy;

   }
   else if( bstatus==4 && linestarted==1 )
   {
      bstatus = 0;
      linestarted = 0;
      makEdge(ox,oy,mousx,mousy);
      struct MassPoint *p1 = edges[edgeCount-1].v1; p1->isFrozen = 1;
      struct MassPoint *p2 = edges[edgeCount-1].v2; p2->isFrozen = 1;
   }


   double blink = rnd();
   setcol(255*blink,255*blink,255*blink);
   if( linestarted==1)
   {
      moveTo(ox,oy);
      lineTo(mousx,mousy);
  }
   }
/* if mouse is clicked, look for closest point, else keep dragging selected point*/
void inputDragPoint(){
   if( bstatus == 2 && grabi == -1 )
   {
      int closestPt = -1;
      double closestD = 1e32;
      for(int i=0; i<pointCount; i++)
      {
         double dSq = ( pts[i].px-mousx)*(pts[i].px-mousx) + (pts[i].py-mousy)*(pts[i].py-mousy);

         if( dSq < closestD ){
            closestD = dSq;
            closestPt = i;
         }
      }

      if( closestD < 10*10)
      {
         grabi = closestPt;
         grabPtr = &pts[closestPt];
         grabPtr->px = mousx; grabPtr->py = mousy;
         grabPtr->ox = mousx; grabPtr->oy = mousy;

         // Find dragged edges
         for(int i=0; i<edgeCount; i++){
             if( edges[i].v1 == grabPtr || edges[i].v2 == grabPtr ){
                 struct Body *b = edges[i].parent;
                 if( b )
                 for(int j=0; j < b->edgeCount; j++){
                     dragEdges[j] = b->edges[j];
                 }
                 break;
             }
         }

      }
   }
   else if(bstatus==0){
      grabi = -1; // release grab
      grabPtr = 0x0;
   }
   else if( grabi > -1) // keep moving
   {
      grabPtr->px = mousx; grabPtr->py = mousy;
      grabPtr->ox = mousx; grabPtr->oy = mousy;
   }
}
/* user has dragged a rect around some space, find masspoints in this area and mark them
   this is used later to make copies or physicsbodies
*/
void findEdgesInRect(int markAsSelected)
{
   for(int i=0; i<edgeCount; i++)
   {
      double wx = mousx-ox;
      double wy = mousy-oy;
      struct MassPoint *v1 = edges[i].v1;
      edges[i].isSelected = 0;
      if( RectCollide( v1->px, v1->py, ox,oy, wx,wy ) )
      {
          edges[i].isSelected = markAsSelected;
          drawSph( v1->px, v1->py, 5);
      }
      struct MassPoint *v2 = edges[i].v2;
      if( RectCollide( v2->px, v2->py, ox,oy, wx,wy ) )
      {
         drawSph( v2->px, v2->py, 5);
      }
   }
}
int RectCollide(double testx,double testy,double rx,double ry,double w,double h) // prolly better ways.
{
   int gotx=0; int goty=0;
   if(w < 0.0 )
   {
      if(testx <= rx && testx >= rx+w) gotx = 1;
   }else{
      if(testx >= rx && testx <= rx+w) gotx = 1;
   }
   if(h < 0.0)
   {
      if(testy <= ry && testy >= ry+h) goty = 1;
   }else{
      if(testy >= ry && testy <= ry+h) goty  = 1;
   }
   if( gotx == 1 && goty == 1 ) return 1;
         return 0;
}

/* Keep in a box. Boring. Make a world with frozen bodies instead. */
void constrainPoints(){
   for(int i=0; i<pointCount; i++)
   {
      // Reset acceleration forces
      pts[i].ax = 0;
      pts[i].ay = -0.5;//-1000;//.0001; // gravity

      if( pts[i].px < 1) pts[i].px = 1;
      if( pts[i].px > xres-1) pts[i].px = xres-1;
      if( pts[i].py < 1) pts[i].py = 1;
      if( pts[i].py > yres-1) pts[i].py = yres-1;
   }
}

/* Makes sure springs are correct length
modify to make stiffer/softer springs, springs that accelerate. springs that shrink/expand.
*/
void updateEdges(){
   for(int i=0; i<edgeCount; i++)
   {
       struct Edge *e = &edges[i];
       int isGrabbingEdge = 0;
       // Check if this edge belongs to a physbody thats being dragged by user.
       for(int j=0; j<MAX_BODY_EDGES; j++){
           if( dragEdges[j] == e ){
               isGrabbingEdge = 1;
               break;
           }
       }

      mp *p1 = edges[i].v1;
      mp *p2 = edges[i].v2;
      // Calculate vector from p2 to p1
      double v1v2x = p1->px - p2->px;
      double v1v2y = p1->py - p2->py;
      double v1v2length = distance2D(v1v2x,v1v2y);
      double diff = v1v2length - edges[i].originalLength;
      //if( fabs(diff) > 1e-7) // Dont bother relaxing if allready pretty relaxed
      {
         // Calc difference from original length
         // normalize v1v2
         v1v2x /= v1v2length;
         v1v2y /= v1v2length;

         // Put both vertices apart by half of the differance respectively
         //int isGrabbingEdge = grabPtr==p1 || grabPtr == p2;


         if( !p1->isFrozen || isGrabbingEdge )
         {
            p1->px -= v1v2x*diff*0.5; p1->py -= v1v2y*diff*0.5;
         }
         if( !p2->isFrozen || isGrabbingEdge)
         {
            p2->px += v1v2x*diff*0.5; p2->py += v1v2y*diff*0.5;
         }
      }
   }
}

// 2d projection is the dot product of the projection axis and the point we want to proj.
void projectToAxis(struct Body *b,double axisx,double axisy,double *mini,double *maxi)
{
   double dotP = axisx * b->verts[0]->px + axisy * b->verts[0]->py;

   // Set max and min values to proj of 1st vert
   *mini = dotP;
   *maxi = dotP;

   for(int i=1; i<b->vertCount; i++)
   {
      dotP = axisx * b->verts[i]->px + axisy * b->verts[i]->py;
      *mini = min( dotP, *mini );
      *maxi = max( dotP, *maxi );
   }
}

/* go thru all phys bodies and check coll. TODO: octree or quadtree or gridmap (broadphase) and bounding rect culling */
void checkCollision(){
   for(int i=0; i<bodyCount; i++)
   {
       struct Body *a = &bodies[i];
       a->hasCollided = 0; // reset collision
       for(int j=0; j<bodyCount; j++)
       {
          if(i==j) continue;
          struct Body *b = &bodies[j];
          if( !(a->isFrozen && b->isFrozen) ) // no point colliding frozen against each other
          if( BodiesOverlap( a,b ) ) { //Test the bounding boxes
              if( detectCollision( a, b ) )
              {
                  handleCollide(a,b);
              }
          }

       }
   }
}

int BodiesOverlap( struct Body* B1, struct Body* B2 )//Simple bounding box overlapping test
{
        return ( B1->MinX <= B2->MaxX ) && ( B1->MinY <= B2->MaxY ) && ( B1->MaxX >= B2->MinX ) && ( B2->MaxY >= B1->MinY );
}

/* move 2 interpenetrating bodies apart by collision depth, opposite dir*/
void handleCollide(struct Body *a, struct Body *b)
{
        a->hasCollided = 1;
        // normalize collision normal
        double len = distance2D(collisionNormalX,collisionNormalY);
        collisionNormalX/=len;
        collisionNormalY/=len;
        // scale normal by collision depth / penetration
        double collisionVectorX = collisionNormalX * collisionDepth;
        double collisionVectorY = collisionNormalY * collisionDepth;

        // Now find the collision edge
        mp *e1 = collisionEdge->v1;
        mp *e2 = collisionEdge->v2;
        //if( pIsFrozen[e1] || pIsFrozen[e2] ) return;
        double T = 0;

        // choose axis (x,y) with largest distance.
        if( fabs(e1->px - e2->px) > fabs(e1->py - e2->py) )
        {
                T = (collisionVert->px - collisionVectorX - e1->px) / (e2->px - e1->px);
        }
        else
        {
                T = (collisionVert->py - collisionVectorY - e1->py) / (e2->py - e1->py);
        }
        double lambda = 1.0/(T*T + (1.0-T)*(1.0-T));

        if( !e1->isFrozen)
        {
                e1->px -= collisionVectorX*(1.0-T)*0.5*lambda;
                e1->py -= collisionVectorY*(1.0-T)*0.5*lambda;
        }
        if( !e2->isFrozen)
        {
                e2->px -= collisionVectorX*( T)*0.5*lambda;
                e2->py -= collisionVectorY*( T)*0.5*lambda;
        }
        // No need to move every vert in a body upon collision
        // just move the colliding vert by half the coll. vector
        if( !collisionVert->isFrozen )
        {
                collisionVert->px += collisionVectorX * 0.5;
                collisionVert->py += collisionVectorY * 0.5;

                // jalla friction
                double vx = collisionVert->ox - collisionVert->px;
                double vy = collisionVert->oy - collisionVert->py;
                collisionVert->ox -= vx/10.0;
                collisionVert->oy -= vy/10.0;

        }
}

/* use SAT to see if two convex polys (bodies) are overlapping */
int detectCollision(struct Body *body1, struct Body *body2){
   collisionVert = 0x0;
    minDistance = 10000.0; // init the length of the collision vector to a large value

   for(int i=0; i<body1->edgeCount + body2->edgeCount; i++)
   {
      struct Edge *edge = 0x0;
      if( i < body1->edgeCount )
         edge = body1->edges[i];
      else edge = body2->edges[ i-body1->edgeCount ];

      // calculate the axis perp to this edge and normalize it
      double axisx = edge->v1->py - edge->v2->py;
      double axisy = edge->v2->px - edge->v1->px;
      double len = distance2D(axisx,axisy);
      axisx/=len; axisy/=len;

      double minA=0; double minB=0; double maxA=0; double maxB=0; // project both bodies onto the perp axis
      projectToAxis(body1, axisx, axisy, &minA, &maxA);
      projectToAxis(body2, axisx, axisy, &minB, &maxB);

      // calc dist between two intervals
      double distance = intervalDistance(minA, maxA, minB, maxB);

      if(distance > 0.0) return 0;

      // if the intervals overlap, check, whether the vector length on this
      // edge is smaller than the smallest length that has been reported so far
      else if( fabs( distance ) < minDistance )
      {
         minDistance = fabs(distance);
         collisionNormalX = axisx;
         collisionNormalY = axisy;
         collisionEdge = edge;
      }
   }
   collisionDepth = minDistance;

   // Detect edge and vert

   // Ensure that the body containing the collision edge lies in B2
   // and the one containing the coll. vert lies in B1
   if(collisionEdgeParent != body2)
   {
      struct Body *temp = body2;
      body2 = body1;
      body1 = temp;
   }

   // This is needed to make sure that the collision normal is pointing at B1
   // calc diff between center points
   // b1.center - b2.center:
   double diffx = body1->center.x - body2->center.x;
   double diffy = body1->center.y - body2->center.y;
   double sign = sgn( collisionNormalX*diffx + collisionNormalY*diffy );

   // Remember that the line equation is N*( R - R0 ). We choose B2.center
   // as R0; the normal N is given by the collision normal

   if( sign != 1 )
   {
      collisionNormalX *= -1;
      collisionNormalY *= -1;
   }

   double smallestD = 1000.0; // init the smallest distance to a high value
   for(int i=0; i< body1->vertCount; i++)
   {
      // measure the dist of the vertex from the line using the line equation
      diffx = body1->verts[i]->px - body2->center.x;
      diffy = body1->verts[i]->py - body2->center.y;
      double distance = collisionNormalX*diffx + collisionNormalY*diffy; // dot prod

      // If the measured distance is smaller than the distance reported
      // so far, set the smallest distance and the collision vector

      if( distance < smallestD )
      {
         smallestD = distance;
         collisionVert = body1->verts[i];
      }
   }

   return 1;
}

/*return the positive distance between to intervals*/
double intervalDistance(double minA,double maxA, double minB,double maxB){
   if(minA < minB) return minB-maxA;
   else return minA - maxB;
}

void calcBodyCenter()
{
    // calc center point of body
    for(int i=0; i<bodyCount; i++)
    {
       struct Body *b = &bodies[i];
       b->center.x = 0; b->center.y = 0;
       b->MinX = 10000.0;
       b->MinY = 10000.0;
       b->MaxX = -10000.0;
       b->MaxY = -10000.0;
       for(int j=0; j<b->vertCount; j++)
       {
          // edges come in pairs. the physBodyEdges array points to e1 and e2
          mp *v = b->verts[j];
          b->center.x += v->px;
          b->center.y += v->py;

          b->MinX = min( b->MinX, v->px );
          b->MinY = min( b->MinY, v->py );
          b->MaxX = max( b->MaxX, v->px );
          b->MaxY = max( b->MaxY, v->py );
       }
       b->center.x /= b->vertCount;
       b->center.y /= b->vertCount;
    }
}

/* update edge spring forces, check collisions. More iterations = stiffer springs */
void IterateCollisions(){
   int phys_iterations = 100;

   for(int it=0; it<phys_iterations; it++)
   {
      calcBodyCenter();
      updateEdges();
      checkCollision();

   }
}

/* Make a box at a given center (x,y) with width and height*/
void makeBox(double x,double y,double w,double h){
   mp *v1 = makPoint(x,y);
   mp *v2 = makPoint(x+w,y);
   mp *v3 = makPoint(x+w,y+h);
   mp *v4 = makPoint(x,y+h);
   makBodyEdge(v1,v2);
   makBodyEdge(v2,v3);
   makBodyEdge(v3,v4);
   makBodyEdge(v4,v1);
   makBodyEdge(v1,v3);
   makBodyEdge(v2,v4);
   makePhysBodyFromSelected();
}

void makeCenteredBox(double x,double y,double s)
{
    mp *v0 = makPoint(x,y);
    mp *v1 = makPoint(x-s,y-s);
    mp *v2 = makPoint(x+s,y-s);
    mp *v3 = makPoint(x+s,y+s);
    mp *v4 = makPoint(x-s,y+s);
    v1->mass = 5;
    makBodyEdge(v1,v0);
    makBodyEdge(v2,v0);
    makBodyEdge(v3,v0);
    makBodyEdge(v4,v0);
    makBodyEdge(v1,v2);
    makBodyEdge(v2,v3);
    makBodyEdge(v3,v4);
    makBodyEdge(v4,v1);
    makBodyEdge(v1,v3);
    makBodyEdge(v2,v4);
    makePhysBodyFromSelected();
}

void makeWheel(double x, double y, double r){
    mp *hub = makPoint(x,y); // center
    mp *first = 0x0;
    int segs = 12;
    mp *prev = 0x0;
    for(int i=0; i<segs; i++){
        double ang0 = (double)i*2.0*M_PI / (double)(segs-1);
        double nx = x+r * cos(ang0); 
	double ny = y+r * sin(ang0);
        mp *v0 = makPoint(nx,ny);
	if(i==0) first = v0;        
	makBodyEdge(hub,v0); // All pts are connected to hub. Wheel Spokes.

        if(i > 0){
            makBodyEdge(prev,v0);
        }
        prev = v0;
    }
    makBodyEdge(prev,first); // close loop

    makePhysBodyFromSelected();
}


